﻿///////////////////////////////////////////////////////////////////////////////
//
//  This file is part of MathLib.NET.
//
//  This library is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.
//  
//  This library is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//  Lesser General Public License for more details.
//  
//  You should have received a copy of the GNU Lesser General Public
//  License along with this library;  If not, see 
//  <http://www.gnu.org/licenses/>.
//
///////////////////////////////////////////////////////////////////////////////

using ILNumerics;
using System;

namespace MathLib
{
    /// <summary>
    ///  Polynomials processing functions
    /// </summary>
    public partial class MLMath
    {
        /// <summary>
        /// Sum of polinomials
        /// NOT IMPLEMENTED IN MATLAB
        /// </summary>
        /// <param name="u">polinomial</param>
        /// <param name="v">polinomial</param>
        /// <returns>Sum</returns>
        public static double[] polysum(double[] u, double[] v)
        {
            int n = u.Length;
            int m = v.Length;
            double[] res;

            if (n == m)
            {
                int i = 0;
                while (i < n && u[i] + v[i] == 0)
                    ++i;

                if (i == n)
                    return new double[1] { 0 };

                res = new double[n - i];
                for (int j = 0; i < n; ++i, ++j)
                    res[j] = u[i] + v[i];
            }
            else if (n > m)
            {
                res = new double[n];
                u.CopyTo(res, 0);

                for (int i = n - m, j = 0; j < m; ++i, ++j)
                    res[i] += v[j];
            }
            else
            {
                res = new double[m];
                v.CopyTo(res, 0);

                for (int i = m - n, j = 0; j < n; ++i, ++j)
                    res[i] += u[j];
            }

            return res;
        }

        /// <summary>
        /// Sum of polinomials
        /// NOT IMPLEMENTED IN MATLAB
        /// </summary>
        /// <param name="u">polinomial</param>
        /// <param name="v">polinomial</param>
        /// <returns>Sum</returns>
        public static complex[] polysum(complex[] u, complex[] v)
        {
            int n = u.Length;
            int m = v.Length;
            complex[] res;

            if (n == m)
            {
                int i = 0;
                while (i < n && u[i] + v[i] == 0)
                    ++i;

                if (i == n)
                    return new complex[1] { new complex(0, 0) };

                res = new complex[n - i];
                for (int j = 0; i < n; ++i, ++j)
                    res[j] = u[i] + v[i];
            }
            else if (n > m)
            {
                res = new complex[n];
                u.CopyTo(res, 0);

                for (int i = n - m, j = 0; j < m; ++i, ++j)
                    res[i] += v[j];
            }
            else
            {
                res = new complex[m];
                v.CopyTo(res, 0);

                for (int i = m - n, j = 0; j < n; ++i, ++j)
                    res[i] += u[j];
            }

            return res;
        }

        /// <summary>
        /// Difference of polinomials
        /// NOT IMPLEMENTED IN MATLAB
        /// </summary>
        /// <param name="u">polinomial</param>
        /// <param name="v">polinomial</param>
        /// <returns>Difference</returns>
        public static double[] polydif(double[] u, double[] v)
        {
            int n = u.Length;
            int m = v.Length;
            double[] res;

            if (n == m)
            {
                int i = 0;
                while (i < n && u[i] - v[i] == 0)
                    ++i;

                if (i == n)
                    return new double[1] { 0 };

                res = new double[n - i];
                for (int j = 0; i < n; ++i, ++j)
                    res[j] = u[i] - v[i];
            }
            else if (n > m)
            {
                res = new double[n];
                u.CopyTo(res, 0);

                for (int i = n - m, j = 0; j < m; ++i, ++j)
                    res[i] -= v[j];
            }
            else
            {
                res = new double[m];
                v.CopyTo(res, 0);

                for (int i = 0; i < m; ++i)
                    res[i] = -res[i];

                for (int i = m - n, j = 0; j < n; ++i, ++j)
                    res[i] += u[j];
            }

            return res;
        }

        /// <summary>
        /// Difference of polinomials
        /// NOT IMPLEMENTED IN MATLAB
        /// </summary>
        /// <param name="u">polinomial</param>
        /// <param name="v">polinomial</param>
        /// <returns>Difference</returns>
        public static complex[] polydif(complex[] u, complex[] v)
        {
            int n = u.Length;
            int m = v.Length;
            complex[] res;

            if (n == m)
            {
                int i = 0;
                while (i < n && u[i] - v[i] == 0)
                    ++i;

                if (i == n)
                    return new complex[1] { new complex(0, 0) };

                res = new complex[n - i];
                for (int j = 0; i < n; ++i, ++j)
                    res[j] = u[i] - v[i];
            }
            else if (n > m)
            {
                res = new complex[n];
                u.CopyTo(res, 0);

                for (int i = n - m, j = 0; j < m; ++i, ++j)
                    res[i] -= v[j];
            }
            else
            {
                res = new complex[m];
                v.CopyTo(res, 0);

                for (int i = 0; i < m; ++i)
                    res[i] = -res[i];

                for (int i = m - n, j = 0; j < n; ++i, ++j)
                    res[i] += u[j];
            }

            return res;
        }

        /// <summary>
        /// Convolution and polinomial multiplication
        /// </summary>
        /// <param name="u">first argument of multiplication</param>
        /// <param name="v">second argument of multiplication</param>
        /// <returns>Convolution</returns>
        public static double[] conv(double[] u, double[] v)
        {
            int n = u.Length;
            int m = v.Length;
            int p = (n - 1) + (m - 1) + 1;
            double[] res = new double[p];

            for (int i = 0; i < p; ++i)
            {
                for (int j = Math.Max(0, -p + i + n); j <= Math.Min(n - 1, m - 1 - p + i + n); ++j)
                {
                    res[i] += u[j] * v[m - 1 - ((p - 1 - i) - (n - 1 - j))];
                }
            }

            return res;
        }

        /// <summary>
        /// Convolution and polinomial multiplication
        /// </summary>
        /// <param name="u">first argument of multiplication</param>
        /// <param name="v">second argument of multiplication</param>
        /// <returns>Convolution</returns>
        public static complex[] conv(complex[] u, complex[] v)
        {
            int n = u.Length;
            int m = v.Length;
            int p = (n - 1) + (m - 1) + 1;
            complex[] res = new complex[p];

            for (int i = 0; i < p; ++i)
            {
                for (int j = Math.Max(0, -p + i + n); j <= Math.Min(n - 1, m - 1 - p + i + n); ++j)
                {
                    res[i] += u[j] * v[m - 1 - ((p - 1 - i) - (n - 1 - j))];
                }
            }

            return res;
        }

        /// <summary>
        /// Integrate polinomial
        /// </summary>
        /// <param name="p">Real array with polinomial coefficients ("aX^2 + bX + c" ~> [a, b, c])</param>
        /// <param name="k">Scalar constant of integration</param>
        /// <returns>Integrated polinomial</returns>
        public static double[] polyint(double[] p, double k = 0)
        {
            int n = p.Length;
            double[] res = new double[n + 1];

            for (int i = 0; i < n; ++i)
            {
                res[i] = p[i] / (n - i);
            }

            res[n] = k;

            return res;
        }

        /// <summary>
        /// Integrate polinomial with integration constant
        /// </summary>
        /// <param name="p">Complex array with polinomial coefficients ("aX^2 + bX + c" ~> [a, b, c])</param>
        /// <param name="k">Scalar constant of integration</param>
        /// <returns>Integrated polinomial</returns>
        public static complex[] polyint(complex[] p, complex k)
        {
            int n = p.Length;
            complex[] res = new complex[n + 1];

            for (int i = 0; i < n; ++i)
            {
                res[i] = p[i] / (n - i);
            }

            res[n] = k;

            return res;
        }

        /// <summary>
        /// Integrate polinomial, integration constant is zero
        /// </summary>
        /// <param name="p">Complex array with polinomial coefficients ("aX^2 + bX + c" ~> [a, b, c])</param>
        /// <param name="k">Scalar constant of integration</param>
        /// <returns>Integrated polinomial</returns>
        public static complex[] polyint(complex[] p)
        {
            int n = p.Length;
            complex[] res = new complex[n + 1];

            for (int i = 0; i < n; ++i)
            {
                res[i] = p[i] / (n - i);
            }

            res[n].imag = 0.0;


            return res;
        }

        /// <summary>
        /// Polinomial derivative
        /// </summary>
        /// <param name="p">Real array with polinomial coefficients ("aX^2 + bX + c" ~> [a, b, c])</param>
        /// <returns>Derivated polinomial</returns>
        public static double[] polyder(double[] p)
        {
            int n = p.Length - 1;
            double[] res = new double[n];

            for (int i = 0; i < n; ++i)
            {
                res[i] = p[i] * (n - i);
            }

            return res;
        }

        /// <summary>
        /// Polinomial derivative
        /// </summary>
        /// <param name="p">Complex array with polinomial coefficients ("aX^2 + bX + c" ~> [a, b, c])</param>
        /// <returns>Derivated polinomial</returns>
        public static complex[] polyder(complex[] p, double k = 0)
        {
            int n = p.Length - 1;
            complex[] res = new complex[n];

            for (int i = 0; i < n; ++i)
            {
                res[i] = p[i] * (n - i);
            }

            return res;
        }

        /// <summary>
        /// Derivative of multiplication of polinomials
        /// </summary>
        /// <param name="p">polinomial</param>
        /// <param name="q">polinomial</param>
        /// <returns>(pq)'</returns>
        public static double[] polyder(double[] a, double[] b)
        {
            return polyder(conv(a, b));
        }

        /// <summary>
        /// Derivative of multiplication of polinomials
        /// </summary>
        /// <param name="p">polinomial</param>
        /// <param name="q">polinomial</param>
        /// <returns>(pq)'</returns>
        public static complex[] polyder(complex[] a, complex[] b)
        {
            return polyder(conv(a, b));
        }

        /// <summary>
        /// Derivative of multiplication of polinomials
        /// </summary>
        /// <param name="p">polinomial</param>
        /// <param name="q">polinomial</param>
        /// <returns>(pq)'</returns>
        public static void polyder(double[] a, double[] b, out double[] p, out double[] q)
        {
            p = polydif(conv(polyder(a), b), conv(a, polyder(b)));
            q = conv(b, b);
        }

        /// <summary>
        /// Derivative of multiplication of polinomials
        /// </summary>
        /// <param name="p">polinomial</param>
        /// <param name="q">polinomial</param>
        /// <returns>(pq)'</returns>
        public static void polyder(complex[] a, complex[] b, out complex[] p, out complex[] q)
        {
            p = polydif(conv(polyder(a), b), conv(a, polyder(b)));
            q = conv(b, b);
        }

        /// <summary>
        /// 1-D interpolation
        /// </summary>
        /// <param name="x">vector holding the x- coordinates of points to be interpolated</param>
        /// <param name="y">vector holding the y- coordinates of points to be interpolated</param>
        /// <param name="xi"> vector holding points of evaluation</param>
        /// <param name="method">interpolation method</param>
        /// <returns>vector holding points of evaluation</returns>
        public static double[] interp1(double[] x, double[] y, double[] xi, string method)
        {
            double[] res = new double[xi.Length];

            if (method == "nearest")
            {
                double[] yi = new double[xi.Length];
                double[] rxi = new double[xi.Length];

                double x_ratio = 1 / (x[1] - x[0]);
                for (int i = 0; i < xi.Length; i++)
                    xi[i] = xi[i] - x[0];

                for (int i = 0; i < xi.Length; i++)
                    rxi[i] = Math.Round(xi[i] * x_ratio, 0, MidpointRounding.AwayFromZero) + 1;

                int index;
                for (int j = 0; j < rxi.Length; j++)
                {
                    index = (int)rxi[j];
                    if (index > 1 && index <= x.Length)
                    {
                        yi[j] = y[index - 1];
                    }
                    else
                        yi[j] = NaN;
                }

                res = yi;
            }
            else if (method == "linear")
            {
                double[] yi = new double[xi.Length];

                for (int i = 0; i < xi.Length; i++)
                {
                    for (int j = 0; j < x.Length - 1; j++)
                    {
                        if (xi[i] >= x[j] && xi[i] < x[j + 1])
                            yi[i] = y[j] + (xi[i] - x[j]) * (y[j + 1] - y[j]) / (x[j + 1] - x[j]);
                    }
                }

                res = yi;
            }
            else if (method == "spline")
            {
                int i, j;
                double[] S = new double[1];
                S[0] = 0.0;

                double[] a = new double[5];
                double[] b = new double[5];
                double[] c = new double[5];
                double[] d = new double[5];
                double[] h = new double[5];
                double[] xa = new double[5];
                double[] xu = new double[5];
                double[] xz = new double[5];
                double[] xl = new double[5];

                for (i = 0; i < 4; i++)
                    a[i] = y[i];

                for (i = 0; i < 4; i++)
                    h[i] = x[i + 1] - x[i];

                for (i = 1; i < 4; i++)
                    xa[i] = 3 * (a[i + 1] * h[i - 1] - a[i] * (h[i - 1] + h[i]) + a[i - 1] * h[i]) / (h[i] * h[i - 1]);

                xl[0] = 1; xu[0] = 0; xz[0] = 0;
                for (i = 1; i < 4; i++)
                {
                    xl[i] = 2 * (h[i - 1] + h[i]) - h[i - 1] * xu[i - 1];
                    xu[i] = h[i] / xl[i];
                    xz[i] = (xa[i] - h[i - 1] * xz[i - 1]) / xl[i];
                }

                xl[4] = 1; xz[4] = 0; c[4] = 0;
                for (i = 0; i < 4; i++)
                {
                    j = 3 - i;
                    c[j] = xz[j] - xu[j] * c[j + 1];
                    b[j] = (a[j + 1] - a[j]) / h[j] - h[j] * (c[j + 1] + 2 * c[j]) / 3;
                    d[j] = (c[j + 1] - c[j]) / (3 * h[j]);
                }

                for (i = 0; i < 4; i++)
                {
                    if (xi[0] >= x[i] && xi[0] < x[i + 1])
                        S[0] = a[i] + b[i] * (xi[0] - x[i]) + c[i] * (xi[0] - x[i]) * (xi[0] - x[i]) + d[i] * (xi[0] - x[i]) * (xi[0] - x[i]) * (xi[0] - x[i]);
                }

                res = S;
            }

            return res;
        }
    }
}
